/*
 Generate a map that can be visualized in rviz

 Minghao Ruan
 */

#include <stdio.h>
#include <string.h>
#include "ros/ros.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/MapMetaData.h"
#include "nav_msgs/OccupancyGrid.h"

class MapNode {
public:
	MapNode() {
		ros::NodeHandle pn("~");
		std::string mpath, frame_id;
		double free_th, occu_th;
		bool invert;

		pn.param("mpath", mpath, std::string("data/map/wean.dat"));
		pn.param("frame_id", frame_id, std::string("map"));
		pn.param("free_th", free_th, 0.5);
		pn.param("occu_th", occu_th, 0.2);
		pn.param("invert", invert, false);

		ROS_INFO("Map path %s", mpath.c_str());
		ROS_INFO("frame_id %s", frame_id.c_str());
		ROS_INFO("Free threshold %.3lf, occupied threshold %.3lf", free_th, occu_th);

		// load the map
		loadmap(map_resp_.map, mpath.c_str(), free_th, occu_th, invert);

		map_resp_.map.info.map_load_time = ros::Time::now();
		map_resp_.map.header.frame_id = frame_id;
		map_resp_.map.header.stamp = ros::Time::now();

		ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
				map_resp_.map.info.width,
				map_resp_.map.info.height,
				map_resp_.map.info.resolution);

		meta_data_msg_ = map_resp_.map.info;

		service = n.advertiseService("static_map", &MapNode::mapCallback, this);

		// Latched publisher for metadata
		metadata_pub = n.advertise<nav_msgs::MapMetaData> ("map_metadata", 1,
				true);
		metadata_pub.publish(meta_data_msg_);

		// Latched publisher for data
		map_pub = n.advertise<nav_msgs::OccupancyGrid> ("map", 1, true);
		map_pub.publish(map_resp_.map);

		ROS_WARN("Done loading map...");
	}

private:
	ros::NodeHandle n;
	ros::Publisher map_pub;
	ros::Publisher metadata_pub;
	ros::ServiceServer service;

	/** The map data is cached here, to be sent out to service callers */
	nav_msgs::MapMetaData meta_data_msg_;
	nav_msgs::GetMap::Response map_resp_;

	bool mapCallback(nav_msgs::GetMap::Request &req,
			nav_msgs::GetMap::Response &res) {
		// request is empty, just ignore it

		// WARN: = operator makes a deep copy (TODO: double check)
		res = map_resp_;
		ROS_INFO("MapNode sending map");
		return true;
	}

	bool loadmap(nav_msgs::OccupancyGrid& map, const char* fname,
			const double free_th, double occu_th, const bool invert) {
		char line[256];
		int size_x, size_y;
		double resolution, offset_x, offset_y;
		FILE* fp;

		if ((fp = fopen(fname, "rt")) == NULL) {
			ROS_ERROR("# Could not open file %s", fname);
			return 0;
		}

		while ((fgets(line, 256, fp) != NULL) && (strncmp("global_map[0]",
				line, 13) != 0))
		{
			if (strncmp(line, "robot_specifications->resolution", 32) == 0)
			{
				if (sscanf(&line[32], "%lf", &resolution) != 0)
					resolution *= .01; // convert cm to meter
				ROS_INFO("# Map resolution: %lf m", resolution);
			}

			if (strncmp(line, "robot_specifications->autoshifted_x", 35) == 0)
			{
				if (sscanf(&line[35], "%lf", &offset_x) != 0)
					ROS_INFO("# Map offsetX: %lf cm", offset_x);
			}
			if (strncmp(line, "robot_specifications->autoshifted_y", 35) == 0)
			{
				if (sscanf(&line[35], "%lf", &offset_y) != 0)
					ROS_INFO("# Map offsetY: %lf cm", offset_y);
			}
		} // end of while

		if (sscanf(line, "global_map[0]: %d %d", &size_y, &size_x) != 2)
		{
			ROS_ERROR(" corrupted file %s", fname);
			return 0;
		}

		map.info.width = size_x;
		map.info.height = size_y;
		map.info.resolution = resolution;
		map.info.origin.position.x = 0;
		map.info.origin.position.y = 0;
		map.info.origin.position.z = 0;
		map.info.origin.orientation.x = 0.0;
		map.info.origin.orientation.y = 0.0;
		map.info.origin.orientation.z = 0.0;
		map.info.origin.orientation.w = 1.0;

		map.data.resize(size_x * size_y);

		double occ;
		int idx;
		for (int y = 0; y < size_y; ++y)
		{
			for (int x = 0; x < size_x; ++x)
			{
				if (fscanf(fp, "%lf", &occ) == 1)
				{
					idx = x * size_x + y;

					if (occ == -1.0)
						map.data[idx] = -1;
					else {
						if (invert)
							occ = 1.0 - occ;

						if (occ < free_th)
							map.data[idx] = 0;
						else if (occ > occu_th)
							map.data[idx] = 100;
						else
							map.data[idx] = -2; // grey area

					}
				} else {
					ROS_ERROR("Error reading in value");
					return 0;
				}
			}
		}// end of filling in data
		return 1;
	}// end of function loadmap
};

int main(int argc, char* argv[]) {
	ros::init(argc, argv, "map_node");

	try {
		MapNode mn;
		ros::spin();
	} catch (std::runtime_error& e) {
		ROS_ERROR("map_server exception: %s", e.what());
		return -1;
	}
	return 0;
}
